import sensor, image, time,pyb,omv,math,utime,tf,lcd,gc
from pyb import UART,Pin,Timer,Servo
from machine import SPI
from ubluetooth import CARSTATE,UBLUETOOTH
from umotor import UMOTOR
from pid import PID
from ultrasonic import ULTRASONIC
pan_servo_offset=-5
global claw_offset
global img
claw_offset=(-20)
claw_release=30
claw_catch=90
pan_servo_start=90+pan_servo_offset
pan_servo_stop=-90
tilt_servo_start=35
tilt_servo_stop=-10
target_title_angle=75
DIS_RATE=27000
global flag_lost
flag_lost=0
tim = Timer(2, freq=50)
claw=tim.channel(3, Timer.PWM, pin=Pin("A2"), pulse_width_percent=7.5)
tracing_serangle_pan=90
tracing_serangle_tilt=60
car_speed=70
pan_servo=Servo(1)
tilt_servo=Servo(2)
pan_servo.calibration(500,2500,1500)
tilt_servo.calibration(500,2500,1500)
pan_pid  = PID(p=0.1, i=0, imax=90)
tilt_pid = PID(p=0.06, i=0.02,d=0.003, imax=90)
dis_pid  = PID(p=0.9, i=0.01)
len_pid  = PID(p=0.9 ,i=0.01)
rho_pid = PID(p=1.2, i=0.5)
theta_pid = PID(p=0.6, i=0.2)
global ultradis,carstate,cardir,left_speed,right_speed,img,THRESHOLD
ultradis=0.0
motor=UMOTOR()
ble=UBLUETOOTH(uart_port=1,baudrate=9600)
ultrawave = ULTRASONIC(trig_pin="B15",echo_pin="B10")
objects = list()
last_objects=objects
def claw_angle(servo_angle):
	if servo_angle<=0:
		servo_angle=0
	if servo_angle>=180:
		servo_angle=180
	percent=(servo_angle+45)/18
	claw.pulse_width_percent(percent)
def servo_move(servo_n,start_angle,end_angle):
	if start_angle>=end_angle :
		while (start_angle>=end_angle) :
			servo_n.angle(start_angle-2)
			time.sleep(50)
			start_angle=start_angle-2
	return 0
	if start_angle<=end_angle :
		while (start_angle<=end_angle) :
			servo_n.angle(start_angle+2)
			time.sleep(50)
			start_angle=start_angle+2
	return 0
def find_max_object(objects,mode=0):
	max_size=0
	if mode==0:
		max_object=None
	if mode==1:
		max_object=(0,0,0,0,0.0)
	for object in objects:
		if object[2]*object[3] > max_size:
			max_object=object
			max_size = object[2]*object[3]
	return max_object
def sensor_judeg_mode(img,s_flag):
	if  s_flag==1:
		s_flag=0
		sensor.set_pixformat(sensor.GRAYSCALE)
		img = sensor.snapshot()
	elif s_flag==2 or s_flag==3 :
		sensor.set_pixformat(sensor.RGB565)
		img = sensor.snapshot()
		s_flag=0
def auto_avoidance(objdis):
	car_speed=70
	if(objdis>= 30):
		motor.run(car_speed,car_speed)
		time.sleep(100)
	if objdis<=20 and (objdis>2.0):
		motor.run(-car_speed,-car_speed)
		time.sleep(600)
	if objdis< 30 and (objdis>20):
		motor.run(car_speed,-car_speed)
		time.sleep(1500)
def color_tracing_line(img):
	THRESHOLD = [(4, 31, -24, 21, -11, 29)]
	pan_servo.angle(tracing_serangle_pan)
	tilt_servo.angle(tracing_serangle_tilt)
	ROIS = [
			(10, int(img.height()*2/3), img.width(), int(img.height()/3), 0.7),
			(10, int(img.height()*1/3), img.width(), int(img.height()/3), 0.3),
			(10, 0, img.width(), int(img.height()/3), 0.1)
		   ]
	weight_sum = 0
	for r in ROIS: weight_sum += r[4]
	centroid_sum = 0
	min_x=1000
	max_x=1
	min_y=1000
	max_y=1
	for r in ROIS:
		blobs = img.find_blobs(THRESHOLD, roi=r[0:4], merge=True)
		if blobs:
			max_blob=find_max_object(objects=blobs,mode=0)
			img.draw_rectangle(max_blob.rect(),color = (255,255,255), thickness = 2, fill = False)
			img.draw_cross(max_blob.cx(),max_blob.cy())
			centroid_sum += max_blob.cx() * r[4]
			if r==ROIS[0]:
				min_x=max_blob.cx()
				min_y=max_blob.cy()
			if r==ROIS[2]:
				max_x=max_blob.cx()
				max_y=max_blob.cy()
	center_pos = (centroid_sum / weight_sum)
	rho_err=0
	rho_err=center_pos-img.width()/2
	print("rho_err: %f" %rho_err )
	theta_err= 0
	theta_err = math.atan((max_x-min_x)/(abs(max_y-min_y)+1))
	theta_err = math.degrees(theta_err)
	print("theta_err: %f" % theta_err)
	rho_output=0
	theta_output=0
	output=0
	if (rho_err>-80) and (theta_err>-44.9):
		rho_output = rho_pid.get_pid(rho_err,1)
		theta_output = theta_pid.get_pid(theta_err,1)
		output = rho_output+theta_output
		print("output",output)
		motor.run(car_speed+output, car_speed-output)
	else:
	   motor.run(0,0)
	   pass
def catch_ball(ball_s,title_angle,target_title_angle):
	global count
	if(ball_s>=50 and ball_s<=100) or (title_angle>=target_title_angle):
		motor.run(0,0)
		count=count+1
		if count >=5:
			count=0
			motor.run(0,0)
			print("catch_ball")
			if (title_angle>=target_title_angle):
				tilt_servo.angle(target_title_angle-5)
			else:
				tilt_servo.angle(target_title_angle-10)
			time.sleep(1000)
			claw_angle(claw_catch)
			time.sleep(1000)
			servo_move(tilt_servo,tilt_servo.angle(),tilt_servo_stop)
			time.sleep(1000)
			pan_servo.angle(0)
			time.sleep(1000)
			pan_servo.angle(pan_servo_stop)
			time.sleep(1000)
			claw_angle(claw_release)
			time.sleep(1000)
			pan_servo.angle(0)
			time.sleep(1000)
			pan_servo.angle(pan_servo_start)
			time.sleep(1000)
			tilt_servo.angle(tilt_servo_start)
			time.sleep(1000)
			motor.run(-70,-70)
			time.sleep(2000)
			motor.run(0,0)
s_last=0
s_lowpower=0
def speed_error_control(ball_s,power,high_s,low_s):
	global s_last
	power_s=power
	s_error=s_last-ball_s
	s_last=ball_s
	if s_error>= high_s :
		power_s=power_s-s_error*0.2
	if abs(s_error)<low_s:
		power_s=power_s+5
	return power_s
def traking_ball(ball_s,title_angle,target_title_angle,pan_error):
	global count
	if ((ball_s>100) and (ball_s<2000)) and (title_angle<target_title_angle):
		count=0
		claw_angle(claw_release)
		power=0
		power_s=0
		power_l=0
		if ball_s>300 and ball_s<=2000:
			dis_error=ball_s-300
			power_s=int(dis_pid.get_pid(dis_error,1)/2)
		elif ball_s>100 and ball_s<=300:
			dis_error=ball_s-100
			power_s=int(dis_pid.get_pid(dis_error,1)/2)
		else:
			power_s =50
		power_s=speed_error_control(ball_s,power_s,25,10)
		print("power_s:",power_s)
		if(power_s>80):
			power_s=80
		if(power_s<40):
			power_s=40
		power_l=int(len_pid.get_pid(pan_error,1))
		motor.run(power_s-power_l,power_s+power_l)
def color_detect(img):
	THRESHOLD  =[ (58, 86, 7, 53, 33, 80),(22, 50, 21, 55, 37, 66)]
	global last_objects,objects,flag_lost
	ball_s=0
	max_blob=None
	if last_objects:
		 for b in objects:
			x1 = b[0]-7
			y1 = b[1]-7
			w1 = b[2]+12
			h1 = b[3]+12
		 roi2 = (x1,y1,w1,h1)
		 objects = img.find_blobs(THRESHOLD,roi = roi2,area_threshold=100,merge=True)
		 last_objects = objects
	else:
		objects = img.find_blobs(THRESHOLD,area_threshold=100,merge=True)
		last_objects = objects
	if objects:
		flag_lost=0
		ball_s=0
		max_blob = find_max_object(objects)
		ball_s=DIS_RATE/(max_blob[2]*2)
		img.draw_rectangle(max_blob.rect())
		cx=int(max_blob[0]+max_blob[2]/2)
		cy=int(max_blob[1]+max_blob[3]/2)
		img.draw_cross(cx, cy)
		img.draw_string(cx,cy, "%.2f mm"%(ball_s))
	return ball_s,max_blob
def find_ball_auto_control(img,mode=0):
	global flag_lost
	ball_s=0
	ball_s,max_blob=color_detect(img)
	if ball_s>0:
		pan_error=0
		tilt_output=0
		title_angle=0
		flag_lost=0
		pan_error = img.width()/2-(max_blob[2]/2+max_blob[0])
		tilt_error = (img.height()/2-claw_offset)-(max_blob[3]/2+max_blob[1])
		tilt_output=tilt_pid.get_pid(tilt_error,1)
		title_angle=tilt_servo.angle()-tilt_output
		tilt_servo.angle(title_angle)
		catch_ball(ball_s,title_angle,target_title_angle)
		traking_ball(ball_s,title_angle,target_title_angle,pan_error)
	else :
		claw_angle(claw_release)
		flag_lost=flag_lost+1
		if mode>0 :
			ultradis=ultrawave.get_distance()
		if flag_lost>5:
			flag_lost=0
			if(tilt_servo.angle()>=tilt_servo_start):
				tilt_servo.angle(tilt_servo.angle())
			else :
				tilt_servo.angle(tilt_servo_start)
			if mode>0 :
				auto_avoidance(ultradis)
			else:
				motor.run(0,0)
def car_state_deal(img):
	carstate,cardir,left_speed,right_speed,s_flag=ble.bluetooth_deal()
	sensor_judeg_mode(img,s_flag)
	if (carstate==CARSTATE.enMANUAL):
		motor.run(left_speed,right_speed)
	elif (carstate==CARSTATE.enFLITING or carstate==CARSTATE.enSTRAKING):
		if(cardir==CARSTATE.enRUN):
			tilt_servo.angle(tilt_servo.angle()-4)
		elif(cardir==CARSTATE.enBACK):
			tilt_servo.angle(tilt_servo.angle()+4)
		elif(cardir==CARSTATE.enLEFT):
			pan_servo.angle(pan_servo.angle()+4)
		elif(cardir==CARSTATE.enRIGHT):
			pan_servo.angle(pan_servo.angle()-4)
		elif(cardir==CARSTATE.enRELEASE):
			claw_angle(claw_release)
		elif(cardir==CARSTATE.enCATCH):
			claw_angle(claw_catch)
	elif(carstate==CARSTATE.enTRACING):
		color_tracing_line(img)
	elif(carstate==CARSTATE.enTRAKING):
		find_ball_auto_control(img,mode=0)
	elif(carstate==CARSTATE.enAVOIDING):
		find_ball_auto_control(img,mode=1)
claw_angle(claw_release)
tilt_servo.angle(tilt_servo_start)
pan_servo.angle(pan_servo_start)
sensor.reset()
sensor.set_framesize(sensor.HQVGA)
sensor.set_pixformat(sensor.RGB565)
sensor.skip_frames(50)
sensor.set_auto_gain(False)
sensor.set_auto_whitebal(False)
clock = time.clock()
last_time= time.ticks()
while(True):
  clock.tick()
  img = sensor.snapshot()
  car_state_deal(img=img)
  print(clock.fps(), "fps", end="\n\n")
  gc.collect()